/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "hhdTransform.h"

#include <cmath>

#include "hhdGameObject.h"
#include "hhdTScene.h"

namespace hhd
{
    /** constructors **/

    Transform::Transform()
    {
        _parent = NULL;
        _position = Vector3(0.0f, 0.0f, 0.0f);
        _rotation = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
        _scale = Vector3(1.0f,1.0f,1.0f);

        _world_matrix = new Matrix4();
        _world_matrix->toIdentity();
        _world_up_to_date = false;
    }

    Transform::~Transform()
    {
        delete _world_matrix;
    }

    Transform::Transform(GameObject* in_parent)
    {
        _parent = in_parent;
        _position = Vector3(0.0f, 0.0f, 0.0f);
        _rotation = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
        _scale = Vector3(1.0f,1.0f,1.0f);

        _world_matrix = new Matrix4();
        _world_matrix->toIdentity();
        _world_up_to_date = false;
    }

    Transform::Transform(const Transform& other)
    {
        _parent = NULL;

        _position = other._position;
        _rotation = other._rotation;
        _scale = other._scale;

        _world_matrix = new Matrix4();
        _world_matrix->toIdentity();
        _world_up_to_date = false;
    }

    Transform& Transform::operator=(const Transform& in_other)
    {
        //give us no parent so we cannot be broken
        _parent = NULL;

        //copy data
        _scale = in_other._scale;
        _rotation = in_other._rotation;
        _position = in_other._position;
        _world_up_to_date = false;
        return *this;
    }

    Transform& Transform::copy(const Transform& in_other, GameObject* in_to_be_parent)
    {
        *this = in_other;
        _parent = in_to_be_parent;
        invalidateWorld();
        return *this;
    }

    void Transform::invalidateWorld()
    {
        //invalidate ourself
        _world_up_to_date = false;
        if(!_parent)
        {
            error("this Transform has no parent GameObject");
        }

        //invalidate all of our children
        for(size_t i = 0; i < _parent->gameObjectCount(); i++)
        {
            _parent->getChildGameObject(i)->_transform.invalidateWorld();
        }
    }

    void Transform::initialize(GameObject* in_game_object)
    {
        _parent = in_game_object;
        invalidateWorld();
        //use this to update at init time not first frame
        getWorldMatrix();
    }


    void Transform::setPosition(float x, float y, float z)
    {
        _position.x = x;
        _position.y = y;
        _position.z = z;

        invalidateWorld();
    }

    void Transform::setPosition(const Vector3& in_position)
    {
        _position = in_position;

        invalidateWorld();
    }

    void Transform::setRotation(const Quaternion& rotation)
    {
        _rotation = rotation;

        invalidateWorld();
    }

    void Transform::setOrientation(const Vector3& forward, const Vector3& up)
    {
        _rotation.setFromBasis(forward, up);
        _rotation.normalize();
        invalidateWorld();
    }

    void Transform::setScale(float x, float y, float z)
    {
        _scale.x = x;
        _scale.y = y;
        _scale.z = z;

        invalidateWorld();
    }

    void Transform::setScale( const Vector3& in_scale_triple )
    {
        _scale = in_scale_triple;
        invalidateWorld();
    }

    void Transform::translate(float x, float y, float z)
    {
        _position.x += x;
        _position.y += y;
        _position.z += z;

        invalidateWorld();
    }

    void Transform::translate(const Vector3& in_vec)
    {
        _position += in_vec;

        invalidateWorld();
    }

    void Transform::rotateX(float theta)
    {
        Quaternion to_apply(Vector3::right, theta);
        _rotation =  to_apply * _rotation;

        invalidateWorld();
    }

    void Transform::rotateY(float theta)
    {
        Quaternion to_apply(Vector3::up, theta);
        _rotation = to_apply * _rotation;

        invalidateWorld();
    }

    void Transform::rotateZ(float theta)
    {
        Quaternion to_apply(Vector3::forward, theta);
        _rotation = to_apply * _rotation;

        invalidateWorld();
    }

    void Transform::rotate(float x_theta, float y_theta, float z_theta)
    {
        Quaternion temp;

        if(x_theta != 0.0f)
        {
            _rotation = temp.setFromAxisAngle(Vector3::right, x_theta).mul( _rotation );
        }
        if(y_theta != 0.0f)
        {
            _rotation = temp.setFromAxisAngle(Vector3::up, y_theta).mul( _rotation );
        }
        if(z_theta != 0.0f)
        {
            _rotation = temp.setFromAxisAngle(Vector3::forward, z_theta).mul( _rotation );
        }
        _rotation.normalize();
        invalidateWorld();
    }

    void Transform::setRotationFromEuler(float x_angle, float y_angle, float z_angle)
    {
        //get identity quaternion
        Quaternion new_rotation;
        Quaternion temp;
        //rotate for each axis
        if(x_angle != 0.0f)
        {
            new_rotation = temp.setFromAxisAngle(Vector3::right, x_angle).mul( new_rotation );
        }
        if(y_angle != 0.0f)
        {
            new_rotation = temp.setFromAxisAngle(Vector3::up, y_angle).mul( new_rotation );
        }
        if(z_angle != 0.0f)
        {
            new_rotation = temp.setFromAxisAngle(Vector3::forward, z_angle).mul( new_rotation );
        }
        new_rotation.normalize();
        _rotation = new_rotation;

        invalidateWorld();
    }

    void Transform::rescale(float x, float y, float z)
    {
        _scale.x *= x;
        _scale.y *= y;
        _scale.z *= z;

        invalidateWorld();
    }

    Vector3 Transform::getWorldPosition() const
    {
        return getWorldMatrix().getTranslation();
    }

    const Vector3& Transform::getLocalPosition() const
    {
        return _position;
    }

    const Vector3& Transform::getLocalScale() const
    {
        return _scale;
    }

    const Quaternion& Transform::getRotation() const
    {
        return _rotation;
    }

    Matrix4 Transform::getLocalMatrix() const
    {
        //get Rotation Matrix
        Matrix4 result(_rotation);
        //set scale
        result.scale(_scale);
        //set translation
        result.setTranslation( _position );
        return result;
    }

    const Matrix4& Transform::getWorldMatrix() const
    {
        //if(this->_parent->_name.equals("Test SpotLight Left"))
        //	error();

        Vector3 forward = _rotation.getForward();

        if(_world_up_to_date)
        {
            return *_world_matrix;
        }
        //not up-to-date so we need to recalculate

        //change this to be a while loop not recursive

        //take a look at this is just seems messed up right now
        if(!_parent || !_parent->getParentGameObject())
        {
            _world_up_to_date = true;
            *_world_matrix = getLocalMatrix();
            return *_world_matrix;
        }
        else
        {
            _world_up_to_date = true;
            *_world_matrix = _parent->getParentGameObject()->getTransform().getWorldMatrix() * getLocalMatrix();
            return *_world_matrix;
        }
    }

    Matrix4 Transform::getWorldRotation()
    {
        //get from world
        Matrix4 result(getWorldMatrix());

        //remove translation
        result.setTranslation(Vector3::zero);
        //remove scale
        result.setRight( result.getRight().normalize() );
        result.setUp( result.getUp().normalize() );
        result.setForward( result.getForward().normalize() );

        return result;
    }

    Vector3 Transform::getLocalRotationEuler() const
    {
        return getLocalMatrix().getRotation();
    }

    Vector3 Transform::getForward() const
    {
        return _rotation.getForward();
    }

    Vector3 Transform::getUp() const
    {
        return _rotation.getUp();
    }

    Vector3 Transform::getRight() const
    {
        return _rotation.getRight();
    }
}
